﻿using AntMovement.Core;
using AntMovement.Core.Attributes;
using AntMovement.Core.Domain;
using AntMovement.Core.Entity;
using AntMovement.Core.IParameter;
using AntMovement.Core.Logic;
using AntMovement.Core.Model;
using AntMovement.Parameter;
using AntMovement.Parameter.Model;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace AntMovement.Handle.TPTest
{
    [FlowAttributes(nameof(Calibration), 6)]
    public class Calibration : TaskBase
    {
        #region 轴变量
        private AxisMotion _axisX = default, _axisY = default, _axisR = default, _axisZ = default;

        private Axis _axisEntityY;
        private Axis _axisEntityR;
        private Axis _axisEntityX;
        private Axis _axisEntityZ;
        #endregion

        #region 上升沿
        private bool _homeAlarm = false, _homeRisingEdge = false, _search = false;
        #endregion

        #region 定时器
        private Ton _tonPicture = new Ton();
        private Ton _tonDelay = new Ton();
        private Ton _tonAxis = new Ton();
        #endregion

        #region 点位
        private PointPositionInfoModel _标定点位;
        #endregion

        #region 输入点
        #endregion

        #region 输出点
        #endregion

        #region 报警变量
        private bool _alarmCalibration = false;
        #endregion

        #region 私有变量
        private Task _task = default;
        private TPSocket _tpSocket = null;
        private bool _heartbeat = false;
        private bool _heartbeatStarted = false;
        private int _count = 0, _counter = 0;
        private PointRoundTripModel _tripModel;
        #endregion
        #region 视觉交互
        /// <summary>
        /// 命令
        /// </summary>
        private bool _calibration = false;
        /// <summary>
        /// 坐标
        /// </summary>
        private float x_current, y_current, r_current;
        /// <summary>
        /// 标定位置
        /// </summary>
        private byte position = 1;
        private double _x, _y, _r;
        private long _pulseX, _pulseY, _pulseR;
        private int _stepVI = 0;
        private double _resultCalibration = 0;
        private long[] points = new long[5];
        private bool _negative = false;
        private bool _start = false;
        #endregion
        public Calibration(ITemporaryParameter temporary, IIOParameter iop, IAxisParameter axisp, IDialogHost dialogHost, ILog log)
            : base(temporary, iop, axisp, dialogHost, log)
        {
            _axisX = axisp.GetAxisMotion("X轴");
            _axisY = axisp.GetAxisMotion("Y轴");
            _axisR = axisp.GetAxisMotion("R轴");
            _axisZ = axisp.GetAxisMotion("Z轴");

            _log = log;
            _axisEntityX = axisp.GetAxisEntity("X轴");
            _axisEntityY = axisp.GetAxisEntity("Y轴");
            _axisEntityR = axisp.GetAxisEntity("R轴");
            _axisEntityZ = axisp.GetAxisEntity("Z轴");
            _标定点位 = axisp.GetPointPosition("标定点位");
            _temporary.Calibration = false;
        }

        public override void EmergencyStop()
        {

        }

        private Stopwatch _stopwatch = new Stopwatch();

        public override void Handle()
        {
            if (_task == null && false)
            {
                _task = Task.Run(() => {
                    _tpSocket = new TPSocket("192.168.100.101");
                    _log.Info("启动视觉交互服务...");
                    while (_temporary.Status_Device != Core.Enum.StatusDeviceType.板卡未连接)
                    {
                        try
                        {
                            _heartbeat = _tpSocket.Heartbeat();
                            if (_heartbeat)
                            {
                                if (_calibration)
                                {
                                    _log.Info($"第[{position}]点 开始标定 X-[{x_current}] Y-[{y_current}] R-[{r_current}]");
                                    var dataCalibration = _tpSocket.Calibration(1, x_current, y_current, r_current, position);
                                    _log.Info($"第[{position}]点 标定结果：[{(dataCalibration[1] != 2 ? "成功" : "失败")}]");
                                    _resultCalibration = dataCalibration[1];
                                    if (_resultCalibration == 2)
                                    {
                                        _dialogHost.DialogShow($"第[{position}]点 视觉标定失败，请重新标定...[注意：多次标定失败的请联系视觉供应商]", Core.Enum.LogLevel.Error);
                                        _log.Error("交互", $"第[{position}]点 视觉标定失败，请重新标定...");
                                    }
                                    _x = dataCalibration[2];
                                    _y = dataCalibration[3];
                                    _r = dataCalibration[4];
                                    _pulseX = DistanceToPulse(_x, _axisEntityX);
                                    _pulseY = DistanceToPulse(_y, _axisEntityY);
                                    _pulseR = DistanceToPulse(_r, _axisEntityR);
                                    _log.Info($"第[{position}]点 标定结束 X-[{_x}] Y-[{_y}] R-[{_r}] PulseX-[{_pulseX}] PulseY-[{_pulseY}] PulseR-[{_pulseR}]");
                                    _calibration = false;
                                }
                            }
                        }
                        catch (Exception ex)
                        {
                            _log.Error("异常", $"视觉交互异常：{ex}");
                        }
                        Thread.Sleep(10);
                    }
                    _log.Warning("关闭视觉交互服务...");
                    _tpSocket.Exit();
                });
            }
            _temporary.HomeComplete = _axisX.HomeComplete && _axisR.HomeComplete && _axisY.HomeComplete && _axisZ.HomeComplete;
            
            _temporary.SystemStatus.AlarmStop = _alarmCalibration || _temporary.SystemStatus.AlarmStop;
            if (_temporary.SystemStatus.Auto)
            {

            }
            if (_temporary.SystemStatus.ManualOperation)
            {
                if (_temporary.Calibration)
                {
                    if (!_homeRisingEdge && !_temporary.HomeComplete)
                    {
                        _dialogHost.DialogShow("轴未全部回原，请先回原在进行标定！！！", Core.Enum.LogLevel.Error, hostDialog: Core.Enum.PopUpType.Global);
                        _homeAlarm = true;
                        _temporary.Calibration = false;
                        _homeRisingEdge = true;
                    }
                    if (!_homeAlarm && !_alarmCalibration)
                    {
                        if (_stepVI == 0)
                        {
                            if (PointPositionMove(_标定点位.XPulse + _pulseX, _标定点位.YPulse + _pulseY, _标定点位.RPulse + _pulseR))
                            {
                                x_current = PulseToDistance(_标定点位.XPulse, _axisEntityX);
                                y_current = PulseToDistance(_标定点位.YPulse, _axisEntityY);
                                r_current = PulseToDistance(_标定点位.RPulse, _axisEntityR);
                                _stepVI = 10;
                            }
                        }
                        if (_stepVI == 10)
                        {
                            _tonPicture.Time(500, true);
                            if (_tonPicture.Out)
                            {
                                _calibration = true;
                                _stepVI = 20;
                                _tonPicture.Time(500, false);
                            }
                        }
                        if (_stepVI == 20)
                        {
                            if (!_calibration)
                            {
                                if (_resultCalibration == 2)
                                {
                                    _alarmCalibration = true;
                                }
                                else
                                {
                                    _stepVI = 30;
                                }
                            }
                        }
                        if (_stepVI == 30)
                        {
                            if (_resultCalibration == 3)
                            {
                                position++;
                                _stepVI = 0;
                            }
                            else
                            {
                                _stepVI = 0;
                                position = 1;
                                _temporary.Calibration = false;
                            }
                        }
                    }
                }

                if (_temporary.RunPointRound)
                {
                    if (!_search)
                    {
                        _log.Info($"[{_temporary.NamePointRound}] 开始执行往返运动 次数[{_tripModel.Count}]");
                        _start = true;
                        Axis axisData = _axisParameter.GetAxisEntity(_temporary.NamePointRound);
                        double pe = axisData.UnitConversion.WeeklyItinerary / axisData.UnitConversion.CyclePulse;
                        _tripModel = _temporary.PointRoundTrips.FirstOrDefault(x => x.Name == _temporary.NamePointRound);
                        points[0] = (long)(_tripModel.PointOne / pe);
                        points[1] = (long)(_tripModel.PointTwo / pe);
                        points[2] = (long)(_tripModel.PointThree / pe);
                        points[3] = (long)(_tripModel.PointFour / pe);
                        points[4] = (long)(_tripModel.PointFive / pe);
                        _search = true;
                    }
                    _tonDelay.Time(2000, !_start);
                    if (_tonDelay.Out)
                    {
                        _stopwatch.Restart();
                        _start = true;
                    }
                    if (_start)
                    {
                        switch (_temporary.NamePointRound)
                        {
                            case "X轴":
                                _axisX.Move(points[_counter], 30000);
                                break;
                            case "Y轴":
                                _axisY.Move(points[_counter], 30000);
                                break;
                            case "R轴":
                                _axisR.Move(points[_counter], 30000);
                                break;
                            case "Z轴":
                                _axisZ.Move(points[_counter], 30000);
                                break;
                        }
                        if ((_axisX.ArrivalLocation && (_temporary.NamePointRound == "X轴")) 
                            || (_axisY.ArrivalLocation && (_temporary.NamePointRound == "Y轴"))
                            || (_axisR.ArrivalLocation && (_temporary.NamePointRound == "R轴"))
                            || (_axisZ.ArrivalLocation && (_temporary.NamePointRound == "Z轴")))
                        {
                            if (_counter == 4)
                            {
                                _stopwatch.Stop();
                                _log.Info($"第[{_count}]次 耗时：{_stopwatch.Elapsed.TotalMilliseconds} ms");
                                _negative = true;
                            }
                            if (_counter == 0)
                            {
                                _count++;
                                _start = false;
                                _negative = false;
                            }
                            if (!_negative)
                            {
                                _counter++;
                            }
                            else
                            {
                                _counter--;
                            }
                        }
                    }
                    if (_count == (_tripModel.Count + 1))
                    {
                        _search = false;
                        _counter = 0;
                        _negative = false;
                        _count = 0;
                        _temporary.RunPointRound = false;
                        _log.Info($"[{_temporary.NamePointRound}] 往返运动结束");
                    }
                }
            }
        }

        public override void Resetting()
        {
            _temporary.RunPointRound = false;
            _search = false;
            if (_homeAlarm)
            {
                _homeAlarm = false;
                _homeRisingEdge = false;
            }
            if (_alarmCalibration)
            {
                _tonPicture.Time(500, false);
                _alarmCalibration = false;
                position = 1;
                _stepVI = 0;
            }
        }

        public override void SafeStop()
        {

        }

        #region 坐标计算
        private float PulseToDistance(long pulse, Axis axis)
        {
            double pe = axis.UnitConversion.WeeklyItinerary / axis.UnitConversion.CyclePulse;
            return (float)Math.Round(pe * (double)axis.Status.CurrentPostion, 2, MidpointRounding.AwayFromZero);
        }

        private long DistanceToPulse(double distance, Axis axis)
        {
            double pe = axis.UnitConversion.WeeklyItinerary / axis.UnitConversion.CyclePulse;
            return (long)Math.Round(distance / pe, 2, MidpointRounding.AwayFromZero);
        }

        private bool PointPositionMove(long xPulse, long yPulse, long rPulse)
        {
            _axisX.Move(xPulse, 10000);
            if (_axisEntityX.Status.CurrentPostion < 148000)
            {
                return false;
            }
            _axisY.Move(yPulse, 10000);
            _axisR.Move(rPulse, 10000);
            return _axisX.ArrivalLocation && _axisY.ArrivalLocation && _axisR.ArrivalLocation;
        }
        #endregion
    }
}
